#include "PathPlanner.h"
#include "SuperData.h"
#include "GameWorld.h"
#include "PathEdge.h"
#include "GraphTypes.h"
#include "Message Dispatcher.H"


PathPlanner::PathPlanner(MovingEntity* owner):m_pOwner(owner),
											  m_pNavGraph(WorldMap->GetGraph()),
											  m_pCurrentSearch(NULL)
{
}


int PathPlanner::GetClosestNodeToPosition(Vector2D Pos)
{
	double NodeX = (Pos.x)/32.0;
	double NodeY = (Pos.y)/32.0;
	
	int nodeIndx = int(NodeY)*WorldMap->MapWidth() + int(NodeX);

	return nodeIndx;
}

int PathPlanner::GetClosestLandNode(Vector2D ToPosition, int idx)
{
	Vector2D curPos = WorldMap->GetGraph()->GetNode(idx).Pos();
	ToPosition.Normalize();

	Vector2D increment = ToPosition;

	int curNodeIdx = idx;
	while(WorldMap->GetGraph()->GetNode(curNodeIdx).NotOverLand())
	{
		curPos -= increment; 
		curNodeIdx = GetClosestNodeToPosition(curPos);
	}

	return curNodeIdx;
}
	

void PathPlanner::SmoothEdges(Path& path)
{
	list<PathEdge>::iterator e1, e2;

	e1 = path.m_EdgeList.begin();

	while(e1 != path.m_EdgeList.end())
	{
		e2 = e1;
		++e2;

		while (e2 != path.m_EdgeList.end())
		{
			if (m_pOwner->CanMoveBetween(e1->Source(), e2->Destination()))
			{
				e1->SetDestination(e2->Destination());
				e2 = path.m_EdgeList.erase(++e1, ++e2);
				e1 = e2;
				--e1;
			}
			else
			{
				++e2;
			}
		}

		++e1;
	}
}

void PathPlanner::GetReadyForNewSearch()
{
	//unregister any exiting search with the path manager
	World->GetPathManager()->UnRegister(this);

	// clean up memory used by any existing search
	delete m_pCurrentSearch;
	m_pCurrentSearch = NULL;
}

bool PathPlanner::RequestPathToPosition(Vector2D TargetPos)
{
	GetReadyForNewSearch();

	m_vDestinationPos = TargetPos;

	// If the path to the destination is unobstructed, they can just move directly
	// to it.
//	if(m_pOwner->CanMoveBetween(m_pOwner->Pos(), TargetPos))
//		return true;

	int ClosestNodeToEntity = GetClosestNodeToPosition(m_pOwner->Pos());
	int ClosestNodeToTarget = GetClosestNodeToPosition(TargetPos);


	typedef Graph_SearchAStar_TS<NavGraph, Heuristic_Euclid> AStar;
	m_pCurrentSearch = new AStar(*m_pNavGraph,
									ClosestNodeToEntity,
									ClosestNodeToTarget);

	// and register the search with the path manager
	World->GetPathManager()->Register(this);
	return true;
}

int PathPlanner::CycleOnce() const
{
	assert (m_pCurrentSearch && "<Reaven_PathPlanner::CycleOnce>: No search object instantiated");

	int result = m_pCurrentSearch->CycleOnce();

	// let the entity know of the failure to find a path
//	if (result == target_not_found)
//	{
//		Dispatcher->DispatchMessage(SENDER_IRRELEVANT, m_pOwner->ID(), NoPathAvailable, NULL);
//	}

	// let the entity know a path has been found
	if (result == target_found || result == target_not_found)
	{
		Dispatcher->DispatchMessage(SENDER_IRRELEVANT, m_pOwner->ID(), PathReady, NULL);
	}

	return result;
}

Path PathPlanner::GetPath()
{
	assert(m_pCurrentSearch && "No Current Search");

	list<PathEdge> edge_list = m_pCurrentSearch->GetPathAsPathEdges();

	Path path(edge_list);

	int closest = GetClosestNodeToPosition(m_pOwner->Pos());
	path.m_EdgeList.push_front(PathEdge(m_pOwner->Pos(), WorldMap->GetGraph()->GetNode(closest).Pos()));

	if (m_pCurrentSearch->GetType() == Graph_SearchTimeSliced<GraphEdge>::AStar)
	{
		path.m_EdgeList.push_back(PathEdge(path.m_EdgeList.back().Destination(),
											m_vDestinationPos));
	}

	SmoothEdges(path);

	return path;
}

